#!/usr/bin/env python3

import rospy
from std_msgs.msg import Int32, String
from sound_play.libsoundplay import SoundClient

class ColorDetector:
    def __init__(self):
        rospy.init_node('color_detector_node', anonymous=True)
        
        # 存储最新的模板匹配结果
        self.latest_template = ""
        
        # 初始化语音合成客户端
        self.sound_client = SoundClient()
        rospy.loginfo("SoundClient initialized. Waiting for sound_play node...")
        rospy.sleep(1)  # 给sound_play节点启动时间
        self.voice = 'voice_kal_diphone'  # 设置语音引擎
        self.color_pub = rospy.Publisher('/color_detector_node/color', String, queue_size=10)
        # 订阅语音命令
        rospy.Subscriber("/voice_controller/commands", Int32, self.voice_command_callback)
        
        # 订阅模板匹配结果
        rospy.Subscriber("/multi_template_matcher/detected_color", String, self.template_callback)
        
        rospy.loginfo("Color Detector Node Started. Waiting for commands...")
    
    def template_callback(self, msg):
        """存储最新的模板匹配结果"""
        self.latest_template = msg.data
        rospy.loginfo(f"Received template: {self.latest_template}")
    
    def voice_command_callback(self, msg):
        """处理语音命令"""
        if msg.data == 6:  # 检查是否为命令6
            rospy.loginfo("Received voice command 6: Processing color detection")
            self.process_color_detection()
    
    def process_color_detection(self):
        """处理颜色检测并合成语音"""
        if not self.latest_template:
            rospy.logwarn("No template received yet. Cannot detect colors.")
            self.color_pub.publish("")
            return
        
        # 定义目标颜色列表
        target_colors = ["green", "red", "black"]
        detected_colors = []
        
        # 检查每个目标颜色是否在模板中出现（不区分大小写）
        for color in target_colors:
            if color.lower() in self.latest_template.lower():
                detected_colors.append(color)
        
        # 发布检测结果
        if detected_colors:
            color_text = ", ".join(detected_colors)
            tts_message = f"I see the colors: {color_text}"
            self.color_pub.publish(color_text)
              
            # 使用sound_play播放语音
            self.sound_client.say(tts_message, self.voice)
            rospy.loginfo(f"Playing: {tts_message}")
        else:
            self.sound_client.say("No colors detected")
            self.color_pub.publish("no color")
            rospy.loginfo("No target colors detected in the template.")

if __name__ == '__main__':
    try:
        detector = ColorDetector()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
